from math import sqrt
import numpy as np


def calc_distance(p1, p2):
    """计算两点之间的距离"""
    p = p1 - p2
    return sqrt(p.x() * p.x() + p.y() * p.y())


def calc_distancetoline(point, line):
    """计算点到线段的距离，如果垂直没有在线段上，则计算点到最近端点的距离"""
    p1, p2 = line
    p1 = np.array([p1.x(), p1.y()])
    p2 = np.array([p2.x(), p2.y()])
    p3 = np.array([point.x(), point.y()])
    # 垂足在p2-p1的延长线上，计算p3到p1的距离
    if np.dot((p3 - p1), (p2 - p1)) < 0:
        return np.linalg.norm(p3 - p1)
    # 垂足在p1-p2的延长线上，计算p3到p2的距离
    if np.dot((p3 - p2), (p1 - p2)) < 0:
        return np.linalg.norm(p3 - p2)
    # 线段重合于一点，计算p3到p1的距离
    if np.linalg.norm(p2 - p1) == 0:
        # return np.linalg.norm(p3 -p1)
        return calc_distance(point, line[0])  # downgraded to two points distance
    # 计算平行四边形的面积除以底的长度，得出高即垂直距离
    return np.linalg.norm(np.cross(p2 - p1, p1 - p3)) / np.linalg.norm(p2 - p1)
